#include "glare.math.IntersectionUtils.h"

#include "glare.math.AxisAlignedBox.h"
#include "glare.math.Ray.h"
#include "glare.math.Vector2.h"
#include "glare.math.Vector3Utils.h"

namespace Glare {

	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, const Plane& plane)
	{

		Real denom = plane.normal.dotProduct(ray.getDirection());
		if (NumericUtils::Abs(denom) < std::numeric_limits<Real>::epsilon())
		{
			// Parallel
			return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
		}
		else
		{
			Real nom = plane.normal.dotProduct(ray.getOrigin()) + plane.d;
			Real t = -(nom/denom);
			return std::pair<bool, Real>(t >= 0, t);
		}

	}
	//-----------------------------------------------------------------------
	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, 
		const std::vector<Plane>& planes, bool normalIsOutside)
	{
		std::list<Plane> planesList;
		for (std::vector<Plane>::const_iterator i = planes.begin(); i != planes.end(); ++i)
		{
			planesList.push_back(*i);
		}
		return intersects(ray, planesList, normalIsOutside);
	}
	//-----------------------------------------------------------------------
	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, 
		const std::list<Plane>& planes, bool normalIsOutside)
	{
		std::list<Plane>::const_iterator planeit, planeitend;
		planeitend = planes.end();
		bool allInside = true;
		std::pair<bool, Real> ret;
		std::pair<bool, Real> end;
		ret.first = false;
		ret.second = 0.0f;
		end.first = false;
		end.second = 0;


		// derive side
		// NB we don't pass directly since that would require Plane::Side in 
		// interface, which results in recursive includes since Math is so fundamental
		Plane::Side outside = normalIsOutside ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;

		for (planeit = planes.begin(); planeit != planeitend; ++planeit)
		{
			const Plane& plane = *planeit;
			// is origin outside?
			if (plane.getSide(ray.getOrigin()) == outside)
			{
				allInside = false;
				// Test single plane
				std::pair<bool, Real> planeRes = 
					ray.intersects(plane);
				if (planeRes.first)
				{
					// Ok, we intersected
					ret.first = true;
					// Use the most distant result since convex volume
					ret.second = std::max(ret.second, planeRes.second);
				}
				else
				{
					ret.first =false;
					ret.second=0.0f;
					return ret;
				}
			}
			else
			{
				std::pair<bool, Real> planeRes = 
					ray.intersects(plane);
				if (planeRes.first)
				{
					if( !end.first )
					{
						end.first = true;
						end.second = planeRes.second;
					}
					else
					{
						end.second = std::min( planeRes.second, end.second );
					}

				}

			}
		}

		if (allInside)
		{
			// Intersecting at 0 distance since inside the volume!
			ret.first = true;
			ret.second = 0.0f;
			return ret;
		}

		if( end.first )
		{
			if( end.second < ret.second )
			{
				ret.first = false;
				return ret;
			}
		}
		return ret;
	}
	//-----------------------------------------------------------------------
	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, const Sphere& sphere, 
		bool discardInside)
	{
		const Vector3& raydir = ray.getDirection();
		// Adjust ray origin relative to sphere center
		const Vector3& rayorig = ray.getOrigin() - sphere.getCenter();
		Real radius = sphere.getRadius();

		// Check origin inside first
		if (rayorig.squaredLength() <= radius*radius && discardInside)
		{
			return std::pair<bool, Real>(true, GLARE_REAL_VALUE(0.0));
		}

		// Mmm, quadratics
		// Build coeffs which can be used with std quadratic solver
		// ie t = (-b +/- sqrt(b*b + 4ac)) / 2a
		Real a = raydir.dotProduct(raydir);
		Real b = 2 * rayorig.dotProduct(raydir);
		Real c = rayorig.dotProduct(rayorig) - radius*radius;

		// Calc determinant
		Real d = (b*b) - (4 * a * c);
		if (d < 0)
		{
			// No intersection
			return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
		}
		else
		{
			// BTW, if d=0 there is one intersection, if d > 0 there are 2
			// But we only want the closest one, so that's ok, just use the 
			// '-' version of the solver
			Real t = ( -b - NumericUtils::Sqrt(d) ) / (2 * a);
			if (t < 0)
				t = ( -b + NumericUtils::Sqrt(d) ) / (2 * a);
			return std::pair<bool, Real>(true, t);
		}


	}
	//-----------------------------------------------------------------------
	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, const AxisAlignedBox& box)
	{
		if (box.isNull()) return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
		if (box.isInfinite()) return std::pair<bool, Real>(true, GLARE_REAL_VALUE(0.0));

		Real lowt = 0.0f;
		Real t;
		bool hit = false;
		Vector3 hitpoint;
		const Vector3& min = box.getMinimum();
		const Vector3& max = box.getMaximum();
		const Vector3& rayorig = ray.getOrigin();
		const Vector3& raydir = ray.getDirection();

		// Check origin inside first
		if ( rayorig > min && rayorig < max )
		{
			return std::pair<bool, Real>(true, GLARE_REAL_VALUE(0.0));
		}

		// Check each face in turn, only check closest 3
		// Min x
		if (rayorig.x <= min.x && raydir.x > 0)
		{
			t = (min.x - rayorig.x) / raydir.x;
			if (t >= 0)
			{
				// Substitute t back into ray and check bounds and dist
				hitpoint = rayorig + raydir * t;
				if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
					hitpoint.z >= min.z && hitpoint.z <= max.z &&
					(!hit || t < lowt))
				{
					hit = true;
					lowt = t;
				}
			}
		}
		// Max x
		if (rayorig.x >= max.x && raydir.x < 0)
		{
			t = (max.x - rayorig.x) / raydir.x;
			if (t >= 0)
			{
				// Substitute t back into ray and check bounds and dist
				hitpoint = rayorig + raydir * t;
				if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
					hitpoint.z >= min.z && hitpoint.z <= max.z &&
					(!hit || t < lowt))
				{
					hit = true;
					lowt = t;
				}
			}
		}
		// Min y
		if (rayorig.y <= min.y && raydir.y > 0)
		{
			t = (min.y - rayorig.y) / raydir.y;
			if (t >= 0)
			{
				// Substitute t back into ray and check bounds and dist
				hitpoint = rayorig + raydir * t;
				if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
					hitpoint.z >= min.z && hitpoint.z <= max.z &&
					(!hit || t < lowt))
				{
					hit = true;
					lowt = t;
				}
			}
		}
		// Max y
		if (rayorig.y >= max.y && raydir.y < 0)
		{
			t = (max.y - rayorig.y) / raydir.y;
			if (t >= 0)
			{
				// Substitute t back into ray and check bounds and dist
				hitpoint = rayorig + raydir * t;
				if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
					hitpoint.z >= min.z && hitpoint.z <= max.z &&
					(!hit || t < lowt))
				{
					hit = true;
					lowt = t;
				}
			}
		}
		// Min z
		if (rayorig.z <= min.z && raydir.z > 0)
		{
			t = (min.z - rayorig.z) / raydir.z;
			if (t >= 0)
			{
				// Substitute t back into ray and check bounds and dist
				hitpoint = rayorig + raydir * t;
				if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
					hitpoint.y >= min.y && hitpoint.y <= max.y &&
					(!hit || t < lowt))
				{
					hit = true;
					lowt = t;
				}
			}
		}
		// Max z
		if (rayorig.z >= max.z && raydir.z < 0)
		{
			t = (max.z - rayorig.z) / raydir.z;
			if (t >= 0)
			{
				// Substitute t back into ray and check bounds and dist
				hitpoint = rayorig + raydir * t;
				if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
					hitpoint.y >= min.y && hitpoint.y <= max.y &&
					(!hit || t < lowt))
				{
					hit = true;
					lowt = t;
				}
			}
		}

		return std::pair<bool, Real>(hit, lowt);

	} 
	//-----------------------------------------------------------------------
	bool IntersectionUtils::intersects(const Ray& ray, const AxisAlignedBox& box,
		Real* d1, Real* d2)
	{
		if (box.isNull())
			return false;

		if (box.isInfinite())
		{
			if (d1) *d1 = 0;
			if (d2) *d2 = NumericUtils::POS_INFINITY;
			return true;
		}

		const Vector3& min = box.getMinimum();
		const Vector3& max = box.getMaximum();
		const Vector3& rayorig = ray.getOrigin();
		const Vector3& raydir = ray.getDirection();

		Vector3 absDir;
		absDir[0] = NumericUtils::Abs(raydir[0]);
		absDir[1] = NumericUtils::Abs(raydir[1]);
		absDir[2] = NumericUtils::Abs(raydir[2]);

		// Sort the axis, ensure check minimise floating error axis first
		int imax = 0, imid = 1, imin = 2;
		if (absDir[0] < absDir[2])
		{
			imax = 2;
			imin = 0;
		}
		if (absDir[1] < absDir[imin])
		{
			imid = imin;
			imin = 1;
		}
		else if (absDir[1] > absDir[imax])
		{
			imid = imax;
			imax = 1;
		}

		Real start = 0, end = NumericUtils::POS_INFINITY;

#define _CALC_AXIS(i)                                       \
	do {                                                    \
	Real denom = 1 / raydir[i];                         \
	Real newstart = (min[i] - rayorig[i]) * denom;      \
	Real newend = (max[i] - rayorig[i]) * denom;        \
	if (newstart > newend) std::swap(newstart, newend); \
	if (newstart > end || newend < start) return false; \
	if (newstart > start) start = newstart;             \
	if (newend < end) end = newend;                     \
	} while(0)

		// Check each axis in turn

		_CALC_AXIS(imax);

		if (absDir[imid] < std::numeric_limits<Real>::epsilon())
		{
			// Parallel with middle and minimise axis, check bounds only
			if (rayorig[imid] < min[imid] || rayorig[imid] > max[imid] ||
				rayorig[imin] < min[imin] || rayorig[imin] > max[imin])
				return false;
		}
		else
		{
			_CALC_AXIS(imid);

			if (absDir[imin] < std::numeric_limits<Real>::epsilon())
			{
				// Parallel with minimise axis, check bounds only
				if (rayorig[imin] < min[imin] || rayorig[imin] > max[imin])
					return false;
			}
			else
			{
				_CALC_AXIS(imin);
			}
		}
#undef _CALC_AXIS

		if (d1) *d1 = start;
		if (d2) *d2 = end;

		return true;
	}
	//-----------------------------------------------------------------------
	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, const Vector3& a,
		const Vector3& b, const Vector3& c, const Vector3& normal,
		bool positiveSide, bool negativeSide)
	{
		//
		// Calculate intersection with plane.
		//
		Real t;
		{
			Real denom = normal.dotProduct(ray.getDirection());

			// Check intersect side
			if (denom > + std::numeric_limits<Real>::epsilon())
			{
				if (!negativeSide)
					return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
			}
			else if (denom < - std::numeric_limits<Real>::epsilon())
			{
				if (!positiveSide)
					return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
			}
			else
			{
				// Parallel or triangle area is close to zero when
				// the plane normal not normalised.
				return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
			}

			t = normal.dotProduct(a - ray.getOrigin()) / denom;

			if (t < 0)
			{
				// Intersection is behind origin
				return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
			}
		}

		//
		// Calculate the largest area projection plane in X, Y or Z.
		//
		size_t i0, i1;
		{
			Real n0 = NumericUtils::Abs(normal[0]);
			Real n1 = NumericUtils::Abs(normal[1]);
			Real n2 = NumericUtils::Abs(normal[2]);

			i0 = 1; i1 = 2;
			if (n1 > n2)
			{
				if (n1 > n0) i0 = 0;
			}
			else
			{
				if (n2 > n0) i1 = 0;
			}
		}

		//
		// Check the intersection point is inside the triangle.
		//
		{
			Real u1 = b[i0] - a[i0];
			Real v1 = b[i1] - a[i1];
			Real u2 = c[i0] - a[i0];
			Real v2 = c[i1] - a[i1];
			Real u0 = t * ray.getDirection()[i0] + ray.getOrigin()[i0] - a[i0];
			Real v0 = t * ray.getDirection()[i1] + ray.getOrigin()[i1] - a[i1];

			Real alpha = u0 * v2 - u2 * v0;
			Real beta  = u1 * v0 - u0 * v1;
			Real area  = u1 * v2 - u2 * v1;

			// epsilon to avoid float precision error
			const Real EPSILON = 1e-6f;

			Real tolerance = - EPSILON * area;

			if (area > 0)
			{
				if (alpha < tolerance || beta < tolerance || alpha+beta > area-tolerance)
					return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
			}
			else
			{
				if (alpha > tolerance || beta > tolerance || alpha+beta < area-tolerance)
					return std::pair<bool, Real>(false, GLARE_REAL_VALUE(0.0));
			}
		}

		return std::pair<bool, Real>(true, t);
	}
	//-----------------------------------------------------------------------
	std::pair<bool, Real> IntersectionUtils::intersects(const Ray& ray, const Vector3& a,
		const Vector3& b, const Vector3& c,
		bool positiveSide, bool negativeSide)
	{
		Vector3 normal = Vector3Utils::calculateBasicFaceNormalWithoutNormalize(a, b, c);
		return intersects(ray, a, b, c, normal, positiveSide, negativeSide);
	}
	//-----------------------------------------------------------------------
	bool IntersectionUtils::intersects(const Sphere& sphere, const AxisAlignedBox& box)
	{
		if (box.isNull()) return false;
		if (box.isInfinite()) return true;

		// Use splitting planes
		const Vector3& center = sphere.getCenter();
		Real radius = sphere.getRadius();
		const Vector3& min = box.getMinimum();
		const Vector3& max = box.getMaximum();

		// Arvo's algorithm
		Real s, d = 0;
		for (int i = 0; i < 3; ++i)
		{
			if (center.ptr()[i] < min.ptr()[i])
			{
				s = center.ptr()[i] - min.ptr()[i];
				d += s * s; 
			}
			else if(center.ptr()[i] > max.ptr()[i])
			{
				s = center.ptr()[i] - max.ptr()[i];
				d += s * s; 
			}
		}
		return d <= radius * radius;

	}
	//-----------------------------------------------------------------------
	bool IntersectionUtils::intersects(const Plane& plane, const AxisAlignedBox& box)
	{
		return (plane.getSide(box) == Plane::BOTH_SIDE);
	}
	//-----------------------------------------------------------------------
	bool IntersectionUtils::intersects(const Sphere& sphere, const Plane& plane)
	{
		return (
			NumericUtils::Abs(plane.getDistance(sphere.getCenter()))
			<= sphere.getRadius() );
	}
	//-----------------------------------------------------------------------
	bool IntersectionUtils::pointInTri2D(const Vector2& p, const Vector2& a, 
		const Vector2& b, const Vector2& c)
	{
		// Winding must be consistent from all edges for point to be inside
		Vector2 v1, v2;
		Real dot[3];
		bool zeroDot[3];

		v1 = b - a;
		v2 = p - a;

		// Note we don't care about normalisation here since sign is all we need
		// It means we don't have to worry about magnitude of cross products either
		dot[0] = v1.crossProduct(v2);
		zeroDot[0] = NumericUtils::RealEqual(dot[0], 0.0f, GLARE_REAL_VALUE(1e-3));


		v1 = c - b;
		v2 = p - b;

		dot[1] = v1.crossProduct(v2);
		zeroDot[1] = NumericUtils::RealEqual(dot[1], 0.0f, GLARE_REAL_VALUE(1e-3));

		// Compare signs (ignore colinear / coincident points)
		if(!zeroDot[0] && !zeroDot[1] 
		&& NumericUtils::Sign(dot[0]) != NumericUtils::Sign(dot[1]))
		{
			return false;
		}

		v1 = a - c;
		v2 = p - c;

		dot[2] = v1.crossProduct(v2);
		zeroDot[2] = NumericUtils::RealEqual(dot[2], 0.0f, GLARE_REAL_VALUE(1e-3));
		// Compare signs (ignore colinear / coincident points)
		if((!zeroDot[0] && !zeroDot[2] 
		&& NumericUtils::Sign(dot[0]) != NumericUtils::Sign(dot[2])) ||
			(!zeroDot[1] && !zeroDot[2] 
		&& NumericUtils::Sign(dot[1]) != NumericUtils::Sign(dot[2])))
		{
			return false;
		}


		return true;
	}
	//-----------------------------------------------------------------------
	bool IntersectionUtils::pointInTri3D(const Vector3& p, const Vector3& a, 
		const Vector3& b, const Vector3& c, const Vector3& normal)
	{
		// Winding must be consistent from all edges for point to be inside
		Vector3 v1, v2;
		Real dot[3];
		bool zeroDot[3];

		v1 = b - a;
		v2 = p - a;

		// Note we don't care about normalisation here since sign is all we need
		// It means we don't have to worry about magnitude of cross products either
		dot[0] = v1.crossProduct(v2).dotProduct(normal);
		zeroDot[0] = NumericUtils::RealEqual(dot[0], 0.0f, GLARE_REAL_VALUE(1e-3));


		v1 = c - b;
		v2 = p - b;

		dot[1] = v1.crossProduct(v2).dotProduct(normal);
		zeroDot[1] = NumericUtils::RealEqual(dot[1], 0.0f, GLARE_REAL_VALUE(1e-3));

		// Compare signs (ignore colinear / coincident points)
		if(!zeroDot[0] && !zeroDot[1] 
		&& NumericUtils::Sign(dot[0]) != NumericUtils::Sign(dot[1]))
		{
			return false;
		}

		v1 = a - c;
		v2 = p - c;

		dot[2] = v1.crossProduct(v2).dotProduct(normal);
		zeroDot[2] = NumericUtils::RealEqual(dot[2], 0.0f, GLARE_REAL_VALUE(1e-3));
		// Compare signs (ignore colinear / coincident points)
		if((!zeroDot[0] && !zeroDot[2] 
		&& NumericUtils::Sign(dot[0]) != NumericUtils::Sign(dot[2])) ||
			(!zeroDot[1] && !zeroDot[2] 
		&& NumericUtils::Sign(dot[1]) != NumericUtils::Sign(dot[2])))
		{
			return false;
		}


		return true;
	}
	//-----------------------------------------------------------------------
}